
#include "../includes.h"

Offset_2nd_ Offset_2nd;

char func_PhaseJump_After;
char func_PhaseJump_Before;
char func_Calc_Offset_2nd;


char PhaseJump_EVENT;
int  PhaseJump_Counter;

double gSumY_Before;
double gSumXDiffY_Before;
double gSumY_After;
double gSumXDiffY_After;
double gOutputData_Before_Offset;

void Init_Offset_2nd()
{
	int i;

	for(i=0;i<OFFSET_WINDOW_SIZE;i++){
		Offset_2nd.DATA[i] = 0;
	}

	Offset_2nd.SumX = 0;
	for(i=0;i<OFFSET_WINDOW_SIZE/2-1;i++){
		Offset_2nd.SumX += i;
	}

	Offset_2nd.XMean = Offset_2nd.SumX/(OFFSET_WINDOW_SIZE/2-1);


	Offset_2nd.SumXVar = 0;

	for(i=0;i<OFFSET_WINDOW_SIZE/2-1;i++){
		Offset_2nd.SumXVar += (i-Offset_2nd.XMean)*(i-Offset_2nd.XMean);
	}

	Offset_2nd.SumY_Before = Offset_2nd.SumY_After = 0;
	Offset_2nd.SumXDiffY_Before = Offset_2nd.SumXDiffY_After = 0;

	Offset_2nd.Grad_Before = Offset_2nd.Grad_After = 0;
	Offset_2nd.YInt_Before = Offset_2nd.YInt_After = 0;

	Offset_2nd.Offset = 0.0;
	Offset_2nd.Offset_Sum = 0.0;
	Offset_2nd.YValue_Before = 0;
	Offset_2nd.YValue_After = 0;

	Offset_2nd.PosEnd = Offset_2nd.Counter = 0;
	Offset_2nd.Calc_Index = 0;
	Offset_2nd.Over8E6 = OFF;

	func_PhaseJump_After = OFF;
	func_PhaseJump_Before = OFF;
	func_Calc_Offset_2nd = OFF;

	PhaseJump_EVENT = OFF;
	PhaseJump_Counter = 0;

	gSumY_Before = gSumXDiffY_Before = gSumY_After = gSumXDiffY_After = 0;

	Offset_2nd.Phase_Jump_Index = 0;
	Offset_2nd.Offset_Add_Flag = OFF;
	gOutputData_Before_Offset = 0;

}

void Push_Offset_2nd(double Data)
{
	if(Offset_2nd.Over8E6){
		if(Data<2E6){
			Data += 1E7;
		}
	}

	if(Offset_2nd.Counter<OFFSET_WINDOW_SIZE){
		Offset_2nd.PosEnd = Offset_2nd.Counter;
		Offset_2nd.DATA[Offset_2nd.PosEnd] = Data;
		Offset_2nd.Counter++;
	}
	else{
		Offset_2nd.Counter = OFFSET_WINDOW_SIZE;


		if(Offset_2nd.PosEnd >= (OFFSET_WINDOW_SIZE-1)){
			Offset_2nd.PosEnd = 0;
		}
		else{
			Offset_2nd.PosEnd++;
		}
		Offset_2nd.DATA[Offset_2nd.PosEnd] = Data;
	}
}

double Pop_Offset_2nd()
{
	double ret_Data;
	int index;

	index = Offset_2nd.PosEnd + OFFSET_WINDOW_SIZE/2;
	if(index>=OFFSET_WINDOW_SIZE){
		index -= OFFSET_WINDOW_SIZE;
	}

	if(Offset_2nd.Counter<OFFSET_WINDOW_SIZE){

		ret_Data = Offset_2nd.DATA[Offset_2nd.PosEnd];
	}
	else{

		ret_Data = Offset_2nd.DATA[index] - Offset_2nd.Offset_Sum;
		gOutputData_Before_Offset = Offset_2nd.DATA[index];
	}


	if(ret_Data>10E6) ret_Data -= 10E6;
	else if( ret_Data <0) ret_Data += 10E6;

	return ret_Data;
}

void Calc_Grad_YInt(char mode)
{
	int Diff;
	int k;

	#define CALC_NUM 5

	if(mode == MODE_PHASEJUMP_BEFORE){


		Diff = Offset_2nd.PosEnd - Offset_2nd.Calc_Index;
		if(Diff<0) Diff += OFFSET_WINDOW_SIZE;

		if(Diff<(OFFSET_WINDOW_SIZE/2)){
			for(k=0;(k<CALC_NUM)&&(Diff<(OFFSET_WINDOW_SIZE/2));k++){

				Offset_2nd.SumY_Before += Offset_2nd.DATA[Offset_2nd.Calc_Index];

				Offset_2nd.SumXDiffY_Before += ((OFFSET_WINDOW_SIZE/2-1-Diff)-Offset_2nd.XMean)*Offset_2nd.DATA[Offset_2nd.Calc_Index--];
				if(Offset_2nd.Calc_Index<0) {
					Offset_2nd.Calc_Index = OFFSET_WINDOW_SIZE-1;
				}

				Diff = Offset_2nd.PosEnd - Offset_2nd.Calc_Index;
				if(Diff<0) Diff += OFFSET_WINDOW_SIZE;
			}
		}

		else {
			Offset_2nd.Grad_Before = Offset_2nd.SumXDiffY_Before/Offset_2nd.SumXVar;
			Offset_2nd.YInt_Before = (Offset_2nd.SumY_Before-Offset_2nd.SumX*Offset_2nd.Grad_Before)/(OFFSET_WINDOW_SIZE/2-1);
			Offset_2nd.YValue_Before = Offset_2nd.Grad_Before*(OFFSET_WINDOW_SIZE/2-1) + Offset_2nd.YInt_Before;
			func_PhaseJump_Before = OFF;


			gSumY_Before = Offset_2nd.SumY_Before;
			gSumXDiffY_Before = Offset_2nd.SumXDiffY_Before;

			Offset_2nd.SumY_Before = Offset_2nd.SumXDiffY_Before = 0;

		}

	}

	else if(mode == MODE_PHASEJUMP_AFTER){
		Diff = Offset_2nd.PosEnd - Offset_2nd.Calc_Index;
		if(Diff<0) Diff += OFFSET_WINDOW_SIZE;

		if(Diff<(OFFSET_WINDOW_SIZE/2)){
			for(k=0;(k<CALC_NUM)&&(Diff<(OFFSET_WINDOW_SIZE/2));k++){

				Offset_2nd.SumY_After += Offset_2nd.DATA[Offset_2nd.Calc_Index];
				Offset_2nd.SumXDiffY_After += ((OFFSET_WINDOW_SIZE/2-1-Diff)-Offset_2nd.XMean)*Offset_2nd.DATA[Offset_2nd.Calc_Index--];
				if(Offset_2nd.Calc_Index<0){
					Offset_2nd.Calc_Index = OFFSET_WINDOW_SIZE-1;
				}

				Diff = Offset_2nd.PosEnd - Offset_2nd.Calc_Index;
				if(Diff<0) Diff += OFFSET_WINDOW_SIZE;
			}
		}

		else {

			Offset_2nd.Grad_After = Offset_2nd.SumXDiffY_After/Offset_2nd.SumXVar;
			Offset_2nd.YInt_After = (Offset_2nd.SumY_After-Offset_2nd.SumX*Offset_2nd.Grad_After)/(OFFSET_WINDOW_SIZE/2-1);
			Offset_2nd.YValue_After = Offset_2nd.YInt_After;
			func_PhaseJump_After = OFF;


			gSumY_After = Offset_2nd.SumY_After;
			gSumXDiffY_After = Offset_2nd.SumXDiffY_After;

			Offset_2nd.SumY_After = Offset_2nd.SumXDiffY_After = 0;
			func_Calc_Offset_2nd = ON;
		}
	}
}


void IsOver8E6()
{
	int i;
	double MaxData = 0.0;

	for(i=0;i<OFFSET_WINDOW_SIZE;i++){
		if(Offset_2nd.DATA[i]>MaxData){
			MaxData = Offset_2nd.DATA[i];
		}
	}

	if(MaxData>8E6){
		Offset_2nd.Over8E6 = ON;
	}
	else Offset_2nd.Over8E6 = OFF;
}

void Calc_Offset_2nd()
{
	Offset_2nd.Offset = Offset_2nd.YValue_After - Offset_2nd.YValue_Before;

	Offset_2nd.Offset_Sum += Offset_2nd.Offset;
    Offset_2nd.Offset_Sum = 0;

	Offset_2nd.Offset_Add_Flag = ON;
}


